/* 
 * File:   ExactBeliefUpdate.h
 * Author: Hadrien
 *
 * Created on 11 juillet 2012, 14:37
 */

#ifndef EXACTBELIEFUPDATE_H
#define	EXACTBELIEFUPDATE_H

template<typename BELIEF>
class ExactBeliefUpdate {
public:
    typedef BELIEF belief_t;
    typedef typename belief_t::super_state_t super_state_t;
    typedef typename super_state_t::state_t state_t;
    typedef typename super_state_t::obs_t obs_t;
    typedef typename super_state_t::action_t action_t;
    ExactBeliefUpdate(){
        
    }
    virtual ~ExactBeliefUpdate(){
        
    }
    
     belief_t operator()(const belief_t& b, const action_t& a, const obs_t& z, const int& K) const{
         return (*this)(b,a,z);
     }
    
    belief_t operator()(const belief_t& b, const action_t& a, const obs_t& z) const{
        //std::cout << "\n---- Belief Update ( action = " << a.index() << ", obs = " << z.index() << " ) ----" << std::endl;
        //std::cout << "%% Old Belief:\n" << b << std::endl;
        belief_t newBelief;
        vector Sb = b.getStates();
        double sum = 0;
        int a_index = a.index();
        int z_index = z.index();
        for(vector_it it = Sb.begin(); it != Sb.end(); ++it) {
            int s = it->getState().index();
            for(int ss = 0; ss < state_t::size; ss++) {
                super_state_t newSState = it->update(a_index,ss,z_index);
                double inc = b.get(*it) * it->getT(s,a_index,ss) * it->getO(ss,a_index,z_index);
                //std::cout << "Inc = " << inc << ", T = " << it->getT(s,a_index,ss) << ", O = " << it->getO(ss,a_index,z_index) << std::endl;
                if(inc>0){
                    newBelief.set(newSState, newBelief.get(newSState) + inc);
                    sum += inc;
                }
            }
        }
        //Normalized        
        newBelief.normalize(sum);
        
        //std::cout << "%% New Belief:\n" << newBelief << std::endl;
        return newBelief;
    }
private:
    typedef typename std::vector<super_state_t> vector;
    typedef typename vector::const_iterator vector_it;

};

#endif	/* EXACTBELIEFUPDATE_H */

